using System;
using System.IO;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

namespace Autobot
{
    public enum BlockState : int
    {
        UNKNOWN = 0,
        BLOCKED = 1,
        FREE = 2,
        OUT_OF_BOUNDS = 3,
        NUM_STATES = 4
    }
    public class Block
    {
        private Vector2 Center;
        private float Height, Width;
        private BlockState State;
       
        public Vector2 BlockCenter
        {
            get{ return Center; }
            set { Center = value; }
        }
        public float BlockHeight
        {
            get { return Height; }
            set { Height = value; }
        }

        public float BlockWidth
        {
            get { return Width; }
            set { Width = value; }
        }

        public BlockState CurrentState
        {
            get { return State; }
            set { State = value; }
        }

        public Block(Vector2 InitCenter, uint InitHeight, uint InitWidth, BlockState InitState)
        {
            Center = InitCenter;
            Height = InitHeight;
            Width = InitWidth;
            State = InitState;
        }
    }

    public class VirtualMap
    {
        //Hold the real map size, map size in pixels, real block size, block size in pixels
        private Vector2 realMapSize, virtualMapSize, realBlockSize, virtualBlockSize, mapSizeInBlocks;
        // Conversion factor for pixels per meter
        private Vector2 pixelsPerMeter;

        // Graphical stuff
        // Sprite batch for drawing
        private Texture2D[] mapSprites;
        private SpriteBatch mapBatch;

        //The actual map of 'blocks'
        private Block[][] map;

        //This is going to be the rectangle in the window
        //where we are going to draw the map
        private Rectangle mapWindow;

        // Map 0,0 is upper left corner of the screen
        //         -y
        //          |
        //          |
        //   -x----x,y----+x
        //          |
        //          |
        //          +y

        // Flag if this map can be edited in the map editor
        private bool readOnly = false;

        // RealMapSize is the dimensions of the real area (meters)
        // Map Window is where to draw the map in the program window (pixels)
        // RealBlockSize is the resolution of 'blocks' that will be displayed in the window
        public VirtualMap(Vector2 RealMapSize, Vector2 RealBlockSize, Rectangle MapWindow, BlockState DefaultState)
        {   
            // Initialize the needed variables
            realMapSize = RealMapSize;

            //Virtual map size is specified by the rect width and height
            virtualMapSize = new Vector2(MapWindow.Width,MapWindow.Height);

            //Determine the conversion factor
            pixelsPerMeter = new Vector2(virtualMapSize.X / realMapSize.X, virtualMapSize.Y / realMapSize.Y);
            
            //Copy the draw rectangle
            mapWindow = MapWindow;

            //Get the map size in 'blocks'
            mapSizeInBlocks = new Vector2((int)(realMapSize.X / RealBlockSize.X),(int)(realMapSize.Y / RealBlockSize.Y));

            //Get the size of each block in pixels
            virtualBlockSize = new Vector2(virtualMapSize.X / mapSizeInBlocks.X,virtualMapSize.Y / mapSizeInBlocks.Y);
            
            //Get the size of each block in meters
            realBlockSize = RealBlockSize;

            // Finally construct the map
            InitMap(DefaultState);
        }

        //Constructor to load a map right off the bat
        public VirtualMap(string Filename, Rectangle MapWindow)
        {
            LoadMap(Filename, MapWindow);
        }

        // Get the real size of the map, used for converting position datas
        public Vector2 GetRealSize()
        {
            return realMapSize;
        }
        /// <summary>
        /// ////////////////////////////// INTERFACE METHODS/////////////////////////////////////
        /// </summary>
    
        public void Update(GameTime gameTime, float[] SensorData, Vector2 Position, Vector2 Direction)
        {
            // We now need to update the map blocks, only if new sensor data is present, to be more accurate
            if (SensorData != null)
            {
                for (int i = 0; i < (int)SensorID.NUM_SENSORS; i++)
                {
                    Block[] LineOfSight = null;
                    int NumBlocks = 0;
                    Vector2 NewDirection = new Vector2(Direction.X,Direction.Y);
                    switch (i)
                    {
                        case (int)SensorID.LEFT:
                            // Assuming we have valid sensor data, then change all the blocks from car to sensor
                            // reading distance to green
                            if (SensorData[i] >= (float)SensorParams.MIN && SensorData[i] <= (float)SensorParams.MAX)
                            {
                                // The left sensor is angled at let's say 45 degrees higher than the direction of the 
                                // car. Update the map to reflect this. The car's direction is expected to be normalized
                                double Angle = -Math.PI / 2.3;
                                NewDirection.X = (float)(Math.Cos(Angle) * Direction.X - Math.Sin(Angle) * Direction.Y);
                                NewDirection.Y = (float)(Math.Sin(Angle) * Direction.X + Math.Cos(Angle) * Direction.Y);

                                LineOfSight = this.GetLineOfBlocks(Position, SensorData[i], NewDirection,ref NumBlocks);
                                for (int j = 0; j < NumBlocks; j++)
                                {
                                    LineOfSight[j].CurrentState = BlockState.FREE;
                                }
                                if ( ((Math.Abs(SensorData[i] - (float)SensorParams.MAX))> 0.05) && NumBlocks > 0)
                                    LineOfSight[NumBlocks - 1].CurrentState = BlockState.BLOCKED;
                            }
                            break;
                        case (int)SensorID.RIGHT:
                            // If we have valid sensor data, then change all the blocks from car to sensor
                            // reading distance to green
                            if (SensorData[i] >= (float)SensorParams.MIN && SensorData[i] <= (float)SensorParams.MAX)
                            {
                                // The left sensor is angled at let's say 45 degrees lower than the direction of the 
                                // car. Update the map to reflect this. The car's direction is expected to be normalized
                                double Angle = Math.PI / 2.3; 
                                NewDirection.X = (float)(Math.Cos(Angle) * Direction.X - Math.Sin(Angle) * Direction.Y);
                                NewDirection.Y = (float)(Math.Sin(Angle) * Direction.X + Math.Cos(Angle) * Direction.Y);

                                LineOfSight = this.GetLineOfBlocks(Position, SensorData[i], NewDirection,ref NumBlocks);
                                for (int j = 0; j < NumBlocks; j++)
                                {
                                    LineOfSight[j].CurrentState = BlockState.FREE;
                                }

                                // If the reading is less than the MAX, then there must be something in the way.
                                // Mark it at the tip of the line of blocks.
                                if (((Math.Abs(SensorData[i] - (float)SensorParams.MAX)) > 0.05) && NumBlocks > 0)
                                    LineOfSight[NumBlocks - 1].CurrentState = BlockState.BLOCKED;
                            }
                            break;
                        case (int)SensorID.MIDDLE:
                            // If we have valid sensor data, then change all the blocks from car to sensor
                            // reading distance to green
                            if (SensorData[i] >= (float)SensorParams.MIN && SensorData[i] <= (float)SensorParams.MAX)
                            {
                                LineOfSight = this.GetLineOfBlocks(Position, SensorData[i], Direction, ref NumBlocks);
                                for (int j = 0; j < NumBlocks; j++)
                                {
                                    LineOfSight[j].CurrentState = BlockState.FREE;
                                }

                                // If the reading is less than the MAX, then there must be something in the way.
                                // Mark it at the tip of the line of blocks.
                                if (((Math.Abs(SensorData[i] - (float)SensorParams.MAX)) > 0.05) && NumBlocks > 0)
                                    LineOfSight[NumBlocks - 1].CurrentState = BlockState.BLOCKED;
                            }
                            break;
                        default:
                            throw new Exception("Unknown Sensor ID From VirtualMap Update");
                    }//switch
                }//for
            }//if
        }//Update()
        public void LoadContent(ContentManager Content,GraphicsDevice drawDevice)
        {
            // Create the sprite batch
            mapBatch = new SpriteBatch(drawDevice);

            // Create the array of map sprites
            mapSprites = new Texture2D[(int)BlockState.NUM_STATES];
            mapSprites[(int)BlockState.BLOCKED] = Content.Load<Texture2D>("Blocked");
            mapSprites[(int)BlockState.FREE] = Content.Load<Texture2D>("Free");
            mapSprites[(int)BlockState.UNKNOWN] = Content.Load<Texture2D>("Unknown");
            mapSprites[(int)BlockState.OUT_OF_BOUNDS] = null;

        }
        public void Dispose()
        {
            for (int i = 0; i < (int)BlockState.NUM_STATES; i++)
            {
                //The OutOfBoundsSprite may be null, so we need to check for that
                if ((mapSprites[i]!=null) && (!mapSprites[i].IsDisposed))
                {
                    mapSprites[i].Dispose();
                }
                //Dispose of the sprite batches
                if (!mapBatch.IsDisposed)
                    mapBatch.Dispose();

            }
        }

        public void Draw()
        {
            /////////////////////// Begin drawing to the screen /////////////////////////////////
            mapBatch.Begin(SpriteBlendMode.AlphaBlend);

            //////////////////////// Draw Map ///////////////////////////////////////////////////
            for (uint i = 0; i < mapSizeInBlocks.Y; i++)
            {
                for (uint j = 0; j < mapSizeInBlocks.X; j++)
                {
                    Block CurrentBlock = map[i][j];

                    Rectangle blockRect = new Rectangle(
                    (int)(CurrentBlock.BlockCenter.X - CurrentBlock.BlockWidth / 2),
                    (int)(CurrentBlock.BlockCenter.Y - CurrentBlock.BlockHeight / 2),
                    (int)CurrentBlock.BlockWidth,
                    (int)CurrentBlock.BlockHeight);

                    //Init the current block's texture for the switch statement
                    Texture2D BlockSprite = mapSprites[(int)BlockState.BLOCKED];

                    switch (CurrentBlock.CurrentState)
                    {
                        case BlockState.BLOCKED:
                            BlockSprite = mapSprites[(int)BlockState.BLOCKED];
                            break;
                        case BlockState.FREE:
                            BlockSprite = mapSprites[(int)BlockState.FREE];
                            break;
                        case BlockState.UNKNOWN:
                            BlockSprite = mapSprites[(int)BlockState.UNKNOWN];
                            break;
                    }

                    mapBatch.Draw(BlockSprite, blockRect, Color.White);
                }
            }
            mapBatch.End();
        }
        private void InitMap(BlockState DefaultState)
        {
            //Allocate memory for the virtual map
            map = new Block[(int)mapSizeInBlocks.Y][];
            for(uint k=0; k<(uint)mapSizeInBlocks.Y;k++)
            {
                map[k] = new Block[(int)mapSizeInBlocks.X];
            }

            //Initialize the blocks to Unknown state because the car hasn't been
            // running yet.
            for(uint i=0; i<mapSizeInBlocks.Y;i++)
            {
                for(uint j=0; j<mapSizeInBlocks.X;j++)
                {
                    // **** These numbers all need to be offset by the map window location
                    // specified in the constructor

                    //The center of the block is half way passed the reference point and offset by where the mapWindow is drawn
                    Vector2 BlockCenter = new Vector2(virtualBlockSize.X*((float)j + 0.5f) + mapWindow.X , virtualBlockSize.Y*((float)i+0.5f) + mapWindow.Y);

                    //Let's just set the border blocks to 'blocked' state for now
                    if ((i == mapSizeInBlocks.Y - 1) || (j == mapSizeInBlocks.X - 1) || (i == 0) || (j == 0))
                    {
                        Block NewBlock = new Block(BlockCenter, (uint)virtualBlockSize.Y, (uint)virtualBlockSize.X, BlockState.BLOCKED);
                        map[i][j] = NewBlock;
                    }
                    else
                    {
                        Block NewBlock = new Block(BlockCenter, (uint)virtualBlockSize.Y, (uint)virtualBlockSize.X, DefaultState);
                        map[i][j] = NewBlock;
                    }
                    
                }
            }
        }

        public Vector2 GetRealPosition(Vector2 VirtualPosition)
        {
            //Remember to offset this position by the drawing window for the map
            //Result will be in meters
            Vector2 Result = new Vector2((VirtualPosition.X - mapWindow.X) / pixelsPerMeter.X,  (VirtualPosition.Y - mapWindow.Y) / pixelsPerMeter.Y);
            return Result;
        }
        public Vector2 GetVirtualPosition(Vector2 RealPosition)
        {
            //Remember to offset this position by the drawing window for the map
            //Result will be in pixels
            Vector2 Result = new Vector2((pixelsPerMeter.X * RealPosition.X) + mapWindow.X , (pixelsPerMeter.Y * RealPosition.Y) + mapWindow.Y);
            return Result;
        }

        //Get the real distance magnitude between two virtual points
        public float GetRealDistance(Vector2 VirtualPos1, Vector2 VirtualPos2)
        {
            //Let's get the real positions
            Vector2 Real1 = GetRealPosition(VirtualPos1);
            Vector2 Real2 = GetRealPosition(VirtualPos2);

            // Get the vector differences
            Vector2 Diff = Real2 - Real1;

            // | sqrt( (X1-X2)^2 + (Y1-Y2)^2 ) |
            return Diff.Length();
        }

        // Check boundaries
        public bool CheckBoundaries(Vector2 Index)
        {
            if ((Index.X >= mapSizeInBlocks.X) || (Index.X < 0))
                return false;
            if ((Index.Y >= mapSizeInBlocks.Y) || (Index.Y < 0))
                return false;

            return true;
        }
        public bool CheckBoundaries(Vector2 Position, bool RealPos)
        {
            if (RealPos == true)
            {
                if ((Position.X < 0) || (Position.Y < 0))
                    return false;
                if ((Position.X > realMapSize.X) || (Position.Y > realMapSize.Y))
                    return false;
            }
            else
            {
                if ((Position.X < mapWindow.X) || (Position.Y < mapWindow.Y))
                    return false;
                if ((Position.X > mapWindow.Right) || (Position.Y > mapWindow.Bottom))
                    return false;
            }
            return true;
        }

        public Block GetBlock(int IndexX, int IndexY)
        {
            if(CheckBoundaries(new Vector2(IndexX,IndexY)))
            {
                return map[IndexY][IndexX];
            }
            else
            {
                Block OutOfBoundsBlock = new Block(new Vector2(0,0),0,0,BlockState.OUT_OF_BOUNDS);
                return OutOfBoundsBlock;
            }
        }
        // Find the closest block to the given position, whether virtual or real
        public Block GetClosestBlock(Vector2 Position, bool RealPosition)
        {
            int indexX;
            int indexY;
            if (RealPosition == true)
            {
                //Don't worry about offset here because these are indexes,
                //not real positions
                indexX = (int)(Position.X / realBlockSize.X);
                indexY = (int)(Position.Y / realBlockSize.Y);
            }
            else
            {
                //Need to offset here because of the window location
                indexX = (int)((Position.X - mapWindow.X) / virtualBlockSize.X);
                indexY = (int)((Position.Y - mapWindow.Y) / virtualBlockSize.Y);
            }

            return GetBlock(indexX, indexY);
        }
        public Vector2 GetBlockIndex(Block block)
        {
            //Need to offset here because of the window location
            uint indexX = (uint)((block.BlockCenter.X - mapWindow.X) / virtualBlockSize.X);
            uint indexY = (uint)((block.BlockCenter.Y - mapWindow.Y) / virtualBlockSize.Y);

            return new Vector2((float)indexX, (float)indexY);
        }
         
        // Will return an array of blocks, sorted as closest to the center
        // This can be used to simulate sensor data, given its position, range and direction.
        public Block[] GetLineOfBlocks(Vector2 RealCenter, float RealDistance, Vector2 Direction, ref int NumBlocks)
        {
            // Get the number of blocks to search based on the range length Using Ceiling Function
            NumBlocks = (int)Math.Ceiling(RealDistance / realBlockSize.X) + 1;

            // Prepare an array to return as a result
            Block[] Result = new Block[NumBlocks];

            // Calculate the current SearchAngle
            float Angle = (float)Math.Atan2(Direction.Y, Direction.X);

            // Start Looking from the center point
            Vector2 CurrentRealPoint = RealCenter;

            // Calculate the length to search for the next block
            Vector2 AdditionVector = new Vector2(Direction.X * realBlockSize.X , Direction.Y * realBlockSize.Y);

            float LengthIncrement = AdditionVector.Length();

            // Get the blocks
            uint BlockCount = 0;
            for (float i = 0.0f; BlockCount < NumBlocks; i += LengthIncrement)
            {   
                //Find the closest blocks
                Result[BlockCount] = GetClosestBlock(CurrentRealPoint, true);
                CurrentRealPoint += AdditionVector;
                BlockCount++;
            }
            if (BlockCount != NumBlocks)
                throw new Exception("Bad number of blocks found");
            return Result;
        }

        //This will return the block at the specified point, along with all the blocks
        // belongin to the next layer around it.
        // Example, If Layers = 0, the block at Position will be returned
        //          If Layers = 1, the block at Position will be returned(P), along with the blocks surrounding it(B's)
        //                                                          B B B
        //                                                          B P B
        //                                                          B B B
        public Block[] GetPaddedBlock(Vector2 VirtualPosition, int Layers, ref int NumBlocks)
        {
            //This will be the dimensions of the square of blocks
            int size = Layers * 2 + 1;

            //Set up the result array
            Block[] result = new Block[size*size];

            // Get the index on the map of the closest block to the virtual position
            Vector2 centerIndex = GetBlockIndex(GetClosestBlock(VirtualPosition, false));

            //Start iterating from the top left corner of the square
            Vector2 indexStart = new Vector2(centerIndex.X - Layers, centerIndex.Y - Layers);

            //Set up a block counter
            int blockCount = 0;

            //Set up an iterative loop to find all the blocks that pad n Layers at Position
            for (int j = (int)indexStart.Y; j < (int)indexStart.Y + size; j++)
            {
                for (int i = (int)indexStart.X; i < (int)indexStart.X + size; i++)
                {
                    result[blockCount] = this.GetBlock(i, j);
                    blockCount++;
                }
            }

            //Check if there are enough blocks
            if(blockCount != size*size)
                throw new Exception("Not enough blocks returned from GetPaddedBlock");

            //Update the referenced block count
            NumBlocks = blockCount;
            
            // return the resulting array of blocks
            return result;
        }
        public void SaveMap(string Filename)
        {
            FileStream file = new FileStream(Filename, FileMode.Create, FileAccess.Write);
            BinaryWriter bw = new BinaryWriter(file);

            //Write a magic number for file ID
            bw.Write((uint)0x13370589);

            // Write the read only flag
            bw.Write(this.readOnly);

            //Write the real width and height of the map (Meters)
            bw.Write(this.realMapSize.X);
            bw.Write(this.realMapSize.Y);

            //Write the real size of blocks
            bw.Write(this.realBlockSize.X);
            bw.Write(this.realBlockSize.Y);

            //Write the States of the blocks in the map
            for (int j = 0; j < mapSizeInBlocks.Y; j++)
            {
                for (int i = 0; i < mapSizeInBlocks.X; i++)
                {
                    bw.Write((int)map[j][i].CurrentState);
                }
            }

            //Close the writer and the file
            bw.Close();
            file.Close();
        }

        public void LoadMap(string Filename, Rectangle MapWindow)
        {
            FileStream file = null;
            BinaryReader br = null;
            try
            {

                file = new FileStream(Filename, FileMode.Open, FileAccess.Read);
                br = new BinaryReader(file);
            }
            catch (IOException)
            {
                Popup.Show("Could not open file: " + Filename);
                return;
            }

            //Read a magic number for file ID
            uint ID = br.ReadUInt32();

            //Check to see if the file is the right one
            if(ID != (uint)0x13370589)
            {
                throw(new Exception("Map cannot read this file type"));
            }

            readOnly = br.ReadBoolean();

            //Read the realMapSize     
            realMapSize = new Vector2(br.ReadSingle(), br.ReadSingle());

            //Read the size of each block in meters
            realBlockSize = new Vector2(br.ReadSingle(), br.ReadSingle());

            //Virtual map size is specified by the rect width and height
            virtualMapSize = new Vector2(MapWindow.Width,MapWindow.Height);

            //Determine the conversion factor
            pixelsPerMeter = new Vector2(virtualMapSize.X / realMapSize.X, virtualMapSize.Y / realMapSize.Y);
            
            //Copy the draw rectangle
            mapWindow = MapWindow;

            //Get the map size in 'blocks'
            mapSizeInBlocks = new Vector2((int)(realMapSize.X / realBlockSize.X),(int)(realMapSize.Y / realBlockSize.Y));

            //Get the size of each block in pixels
            virtualBlockSize = new Vector2(virtualMapSize.X / mapSizeInBlocks.X,virtualMapSize.Y / mapSizeInBlocks.Y);
            
            // Finally construct the map
            InitMap(BlockState.UNKNOWN);
            
            //Read the States of the blocks in the map
            for (int j = 0; j < mapSizeInBlocks.Y; j++)
            {
                for (int i = 0; i < mapSizeInBlocks.X; i++)
                {
                    BlockState blockState = (BlockState)br.ReadInt32();
                    map[j][i].CurrentState = blockState;
                }
            }

            //Close the reader and the file
            br.Close();
            file.Close();
        }

        public bool SetReadOnly()
        {
            readOnly = true;
            return readOnly;
        }

        public bool ReadOnly()
        {
            return readOnly;
        }

    }//Class VirtualMap
}// Namespace Autobot
